import socket
import json

import sys
sys.path.append('/home/a/unitree_sdk/unitree_legged_sdk-3.2/scripts')
import Unitree_Python_sdk
import time
unitree_robot = Unitree_Python_sdk.Unitree_Robot_High()


def start(s):
    # senddata = json.dumps([1])  # 0 send, 1 accept, 2 data
    # s.send(bytes(senddata.encode('utf-8')))
    while True:
        try:
            data = s.recv(1024)
            data = json.loads(data)
            # print(data)
            if not data:
                break
        except Exception as e:
            print('Exception: {0}'.format(e))
            break
        finally:
            mode = data[0]
            forwardSpeed = data[1]
            sideSpeed = data[2]
            rotateSpeed = data[3]
            unitree_robot.robot_walking(mode=mode, forwardSpeed=forwardSpeed, sideSpeed=sideSpeed, rotateSpeed=rotateSpeed, bodyHeight=0.0, footRaiseHeight=0.1)


s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
address_server = ('192.168.12.165', 8050) # localhost
s.connect(address_server)
print('Connected.')
start(s)
